#include "coarse_registration.h"
#include <iostream>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>

CoraseRegistration::CoraseRegistration() {}

CoraseRegistration::~CoraseRegistration() {}

void CoraseRegistration::GenerateDescriptor() {
  pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>
      est_fpfh_omp;
  est_fpfh_omp.setNumberOfThreads(4);
  est_fpfh_omp.setInputCloud(dox_->current_sparse_point_cloud.makeShared());
  est_fpfh_omp.setInputNormals(dox_->current_normal);
  est_fpfh_omp.setSearchMethod(dox_->current_kdtree);
  est_fpfh_omp.setKSearch(10);
  est_fpfh_omp.compute(dox_->current_fpfh_feature);
}

void CoraseRegistration::MatchFpfhDescriptor() {

  pcl::CorrespondencesPtr correspondence(new pcl::Correspondences);
  pcl::registration::CorrespondenceEstimation<pcl::FPFHSignature33,
                                              pcl::FPFHSignature33>
      cest;
  cest.setInputSource(dox_->pre_fpfh_feature.makeShared());
  cest.setInputTarget(dox_->current_fpfh_feature.makeShared());
  cest.determineCorrespondences(*correspondence);
  std::cout << "Generate correspondences" << correspondence->size()
            << std::endl;
  pcl::CorrespondencesPtr corr_filtered(new pcl::Correspondences);
  pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ>
      rejector;
  rejector.setInputSource(dox_->pre_sparse_point_cloud.makeShared());
  rejector.setInputTarget(dox_->current_sparse_point_cloud.makeShared());
  rejector.setInlierThreshold(2.5);
  rejector.setMaximumIterations(100);
  rejector.setRefineModel(false);
  rejector.setInputCorrespondences(correspondence);
  rejector.getCorrespondences(*corr_filtered);
  std::cout << "filter correspondences" << corr_filtered->size() << std::endl;
  Eigen::Matrix4f transform;
  pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ>
      trans_est;
  trans_est.estimateRigidTransformation(dox_->pre_sparse_point_cloud,
                                        dox_->current_sparse_point_cloud,
                                        *corr_filtered, transform);
  dox_->init_registration_transform_matrix = transform;
  std::cout << transform << std::endl;
}

void CoraseRegistration::Init(Dox *dox) { dox_ = dox; }
